/***************************************************************************
 *  Filename: main.cpp
 *  Class:
 *
 *  Copyright 2012  Mario Henrique Voorsluys
 *  <forcaeluz@gmail.com>
 *
 ****************************************************************************/


#include <QtCore/QCoreApplication>
#include <QThread>
#include <QObject>
#include <QtCore>
#include <QIODevice>
#include <cstdio>
#include "maincontroller.h"
#include "navadatareceiver.h"
#include "controlsender.h"
#include "dronemodeller.h"

#include <QDebug>
#include <QFile>
#include <QTextStream>
#include <QTcpSocket>

#define LOGLEVEL QtDebugMsg

void myMessageHandler(QtMsgType type, const char *msg)
{
    QString txt;

    switch (type) {
    case QtDebugMsg:
        txt = QString("Debug: %1 %2").arg(QDateTime::currentDateTime().toString(), msg);
        break;
    case QtWarningMsg:
        txt = QString("Warning: %1 %2").arg(QDateTime::currentDateTime().toString(), msg);
        break;
    case QtCriticalMsg:
        txt = QString("Critical: %1").arg(msg);
        break;
    case QtFatalMsg:
        txt = QString("Fatal: %1").arg(msg);
        abort();
    }

    if(type >= LOGLEVEL){
        QFile outFile("/home/mvoorsluys/ardronecontroller.log");
        outFile.open(QIODevice::WriteOnly | QIODevice::Append);
        QTextStream ts(&outFile);
        ts << txt << endl;
    }
}

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);
    QFile logfile("/home/mvoorsluys/ardronecontroller.log");
    logfile.remove();
    //a.addLibraryPath(".");
    qInstallMsgHandler(myMessageHandler);
    qDebug() << WIFI_MYKONOS_IP;
    //Create objects and threads.

    QThread *controlsThread = new QThread(&a);
    ControlSender *ctrlsender = new ControlSender();
    ctrlsender->moveToThread(controlsThread);
    controlsThread->start();
    ctrlsender->startRunning();

    QThread *navdatarThread = new QThread(&a);
    NavDataReceiver *navdatareceiver = new NavDataReceiver();
    navdatareceiver->moveToThread(navdatarThread);
    navdatarThread->start();
    navdatareceiver->initializate();


    QThread *controllerThread = new QThread(&a);
    MainController *controller = new MainController();
    controller->moveToThread(controllerThread);
    controllerThread->start();


    QObject::connect(controller, SIGNAL(updateCommands(float,float,float,float)),
                     ctrlsender, SLOT(updateCommands(float,float,float,float)));
    QObject::connect(navdatareceiver, SIGNAL(updateAltitude(float)),
                     controller, SLOT(updateAltitude(float)));
    QObject::connect(navdatareceiver, SIGNAL(updateAngleData(float,float,float)),
                     controller, SLOT(updateAnglesData(float,float,float)));
    QObject::connect(navdatareceiver, SIGNAL(updateVelocities(float,float,float)),
                     controller, SLOT(updateSpeeds(float,float,float)));


    QThread *modellerThread = new QThread(&a);
    DroneModeller modeller;
    modeller.moveToThread(modellerThread);
    QObject::connect(modellerThread, SIGNAL(started()), &modeller, SLOT(run()));
    modellerThread->start();


    QObject::connect(&modeller, SIGNAL(newTarget(float,float,float, float)),
                     controller, SLOT(updateRelativeTarget(float,float,float,float)));
    QObject::connect(navdatareceiver, SIGNAL(initSequence(int)), ctrlsender, SLOT(sendInitSequence(int)));
    QObject::connect(navdatareceiver, SIGNAL(updateState(uint)), ctrlsender, SLOT(updateState(uint)));
    QObject::connect(&modeller, SIGNAL(startController()), controller, SLOT(controlLoop()));
    QObject::connect(&modeller, SIGNAL(changeFile(QString)), navdatareceiver, SLOT(newFileName(QString)));
    QObject::connect(&modeller, SIGNAL(fly(float,float,float,float)), ctrlsender, SLOT(updateCommands(float,float,float,float)));
    QObject::connect(&modeller, SIGNAL(fly(float,float,float,float)), navdatareceiver, SLOT(desiredValues(float,float,float,float)));
    QObject::connect(&modeller, SIGNAL(takeOff()), ctrlsender, SLOT(takeoff()));
    QObject::connect(&modeller, SIGNAL(land()), ctrlsender, SLOT(land()));
    QObject::connect(&modeller, SIGNAL(reset()), ctrlsender, SLOT(reset()));
    QObject::connect(&modeller, SIGNAL(exit()), &a, SLOT(quit()));
    return a.exec();
}
